# (c) Tashan and Friends 2010

from pylab import sin, cos, arctan, pi, plot, show, title, xlabel, xlim, ylabel, ylim, grid, linspace

class Point():
    """ Define a point in 3 dimension with x, y, and z coordinates. By default its initialised to (0,0,0).
    Example: a = Point(1,2,3)
             x coordinate can be accessed using a.x similarly with y and z.
    To define a origin you can directly write a = Point()

    """
    def __init(self,x = 0,y = 0,z = 0):
        self.x=x
        self.y=y
        self.z=z



class Satellite():

    """Define a object satellite with the following parameters:

    muE =       Gravitation constant of earth
    capWe =     Angular  
    capWedot = 

    n0 =        Mean angular velocity
    deltaN =    Correction to the mean motion computed using rootA
    rootA =     Square root of the semi-major axis
    ecc =       Eccentricity
    i0 =        Inclination angle at the reference time
    capW0 =     Longitude of the ascending node at the beginning of the GPS week
    w =         Argument of perigee
    M0 =        Mean anomaly at the reference time
    IDOT =      The rate of change of the inclination with time
    capW0dot =  The rate of change of the right ascension of the ascending node with time
    
    Cuc, Cus =  Amplitude of correction terms for the computed argument of latitude
    Crc, Crs =  Amplitude of correction terms for the computed orbit radius
    Cic, Cis =  Amplitude of correction terms for the computed inclination angle

    Toe =       Ephemeris reference time
    
    """
    def __init__(self):
        self.muE = 398600.4415
        self.rootA = 12345
        self.A = (self.rootA)**2
        self.capWe = 7.2921151467e-5
        self.n0 = (self.capWe/(self.A**3))**.5
        self.deltaN = .0001
        self.n = self.n0 + self.deltaN
        self.M0 = 1
        self.ecc = .99
        self.Cus = 0
        self.Cuc = 0
        self.Cis = 0
        self.Cic = 0
        self.Crs = 0
        self.Crc = 0
        self.Toe = 0 
        self.capWedot = .122 
        self.capW0dot = .0123
        self.i0 = .1
        self.IDOT = .01 
        self.w = .001
        self.capW0 = .001
    def calcPos(self,t):
        """Calculate position of satellite at any given time t.
           Output : <type> Point
        """
        Toe = self.Toe
        A = self.A
        ecc =self.ecc
        n = self.n
        M0 = self.M0
        Cus = self.Cus
        Cuc = self.Cuc
        Cis = self.Cis
        Cic = self.Cic
        Crs = self.Crs
        Crc = self.Crc
        capWe = self.capWe
        capWedot = self.capWedot
        capW0dot = self.capW0dot
        i0 = self.i0
        IDOT = self.IDOT
        w = self.w
        capW0 = self.capW0

        t = t -Toe
        coor = Point()
        M = self.M0 + n*t
        M = (M + 2*pi)%(2*pi)
        E = self.calcE(M)
        V = arctan((((1-ecc**2)**.5)*sin(E))/(cos(E)-ecc))
        Phi = V + w
        delU = Cus*sin(2*Phi) + Cuc*cos(2*Phi)
        delR = Crs*sin(2*Phi) + Crc*cos(2*Phi)
        delI = Cis*sin(2*Phi) + Cic*cos(2*Phi)
        U = Phi + delU
        R = A*(1-ecc*cos(E)) + delR
        I = i0 + delI + IDOT*t
        xdash = R*cos(U)
        ydash = R*sin(U)
        capW = capW0 + (capW0dot-capWedot)*t -capWedot*Toe
        coor.x = xdash*cos(capW) - ydash*cos(I)*sin(capW)
        coor.y = xdash*sin(capW) + ydash*cos(I)*cos(capW)
        coor.z = ydash*sin(I)
        return coor

    def calcE(self,M):
        """Calculate eccentric anomaly from mean anomaly
        """
        ecc = self.ecc
        E = M
        while 1:
            E_old = E
            E = M + ecc*sin(E)
            dE = (E - E_old)%2*pi
            if abs(dE) < 1e-12:
                break
        return E

    def plotCoordinate(self,t):
        time = linspace(0,t,2*t)
        coorx = []
        coory = []
        coorz = []

        for i in time:
            p= self.calcPos(i)
            coorx.append(p.x)
            coory.append(p.y)
            coorz.append(p.z)
        plot(coorx,coory)
        show()
        
